from pyntcloud import PyntCloud
from pyntcloud.geometry.models.sphere import create_sphere
import open3d as o3d
import matplotlib.pyplot as plt
from b_filter_ground_equ import GroundEqu
from multiprocessing import Pool

import os,copy
import math,pickle
import numpy as np
from scipy.spatial.transform import Rotation as R
class Volume:
    def __init__(self):
        self.g=GroundEqu()
        self.id,self.groundEqu,self.threshhold=self.g.main()
        self.data =self.g.get_all(self.id)
        self.color = self.data["color"]
        self.dicpnts = self.data["pnt"]
    def filter_obj_by_groundequ(self):
        a, b, d = self.groundEqu
        c = -1
        vec3 = np.array([a, b, c])
        down = np.sqrt(np.sum(np.multiply(vec3, vec3)))

        all=np.array([i for i in self.dicpnts.values()])
        rows = all.shape[0]
        vec1 = np.repeat([[a, b, c, d]], rows, axis=0)

        vec2 = np.ones((rows, 4))
        vec2[:, 0:3] = all
        res1 = np.multiply(vec1, vec2)
        up = np.abs(np.sum(res1, axis=1))
        dis = up / down
        # mask = dis >= self.threshhold
        mask = dis >= self.threshhold*5
        obj = all[mask]
        return obj.tolist()
    def project_point2_plane(self,pnt,a,b,c,d):

        right=np.array([
            [1/a,-1/b,0],
            [1/a,0, -1/c],
            [a, b, c],
        ])
        x,y,z=pnt
        left=np.array([x/a-y/b,x/a-z/c,-d])
        res=np.linalg.solve(right,left)
        return res.tolist()

    def get_volume(self,li):
        data = np.asarray(li)
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(data)

        cloud = PyntCloud.from_instance("open3d", pcd)
        convex_hull_id = cloud.add_structure("convex_hull")
        convex_hull = cloud.structures[convex_hull_id]
        print(convex_hull.volume)
        # cloud.plot()
        return convex_hull.volume

    def signed_dis_2ground(self,src):
        a, b, d = self.groundEqu
        c = -1
        x1,y1,z1=src
        up=a*x1+b*y1+c*z1+d
        down=np.sqrt(a*a+b*b+c*c)
        return up/down

    def connnect_src_projection(self,src):
        res=[]
        a, b, d = self.groundEqu
        c = -1
        down=np.sqrt(a*a+b*b+c*c)
        x1,y1,z1=src
        signed_dis=self.signed_dis_2ground(src)
        n=100
        step=signed_dis/n
        lines=[step*(i+1) for i in range(n)]
        for i in lines:
            d1=i*down-(a*x1+b*y1+c*z1)
            projection=self.project_point2_plane(src,a,b,c,d1)
            res.append(projection)
        return res



    def main(self):
        obj=self.filter_obj_by_groundequ()
        with Pool() as p:
            res=p.map(self.connnect_src_projection,obj)

        res=np.asarray(res)
        res1=np.concatenate(res)
        res2=res1.tolist()
        vol=self.get_volume(res2)
        print ("volume=====",vol)
        self.g.vis_pnt([res2])
        return res2

if __name__ == "__main__":
    r =Volume()
    r.main()
    # r.get_pitch_roll()

